#include "MessageSubscriber.h"
#include <fastrtps/attributes/ParticipantAttributes.h>
#include <fastrtps/attributes/SubscriberAttributes.h>
#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
#include <fastdds/dds/subscriber/Subscriber.hpp>
#include <fastdds/dds/subscriber/DataReader.hpp>
#include <fastdds/dds/subscriber/SampleInfo.hpp>
#include <fastdds/dds/subscriber/qos/DataReaderQos.hpp>

#include <fastdds/rtps/transport/shared_mem/SharedMemTransportDescriptor.h>
#include <fastdds/rtps/transport/TCPv4TransportDescriptor.h>
#include <fastdds/rtps/transport/UDPv4TransportDescriptor.h>
#include <fastrtps/utils/IPLocator.h>

using namespace eprosima::fastdds::dds;
using namespace eprosima::fastdds::rtps;
using IPLocator = eprosima::fastrtps::rtps::IPLocator;

#define SEND_BUFFER_SIZE 9216
#define RECV_BUFFER_SIZE 9216

extern time_t getTimestamp();


bool loadXmlFile()
{
    do {
        auto factory = DomainParticipantFactory::get_instance();
        if (ReturnCode_t::RETCODE_OK != factory->load_XML_profiles_file("E:\\code\\DDS_Demo\\dds_xml\\userTransports_xml_profile.xml")) {
            break;
        }
        if (ReturnCode_t::RETCODE_OK != factory->load_XML_profiles_file("E:\\code\\DDS_Demo\\dds_xml\\datareader_xml_profile.xml")) {
            break;
        }
        if (ReturnCode_t::RETCODE_OK != factory->load_XML_profiles_file("E:\\code\\DDS_Demo\\dds_xml\\datawriter_xml_profile.xml")) {
            break;
        }
        if (ReturnCode_t::RETCODE_OK != factory->load_XML_profiles_file("E:\\code\\DDS_Demo\\dds_xml\\domainparticipant_sub_profile.xml")) {
            break;
        }
        std::string static_xml_file = "E:\\code\\DDS_Demo\\dds_xml\\staticdiscovery_sub_xml_profile.xml";
        if (ReturnCode_t::RETCODE_OK != factory->check_xml_static_discovery(static_xml_file)) {
            break;
        }
        //static_xml_file = "E:\\code\\DDS_Demo\\dds_xml\\staticdiscovery_pub_xml_profile.xml";
        //if (ReturnCode_t::RETCODE_OK != factory->check_xml_static_discovery(static_xml_file)) {
           // break;
        //}

       // if (ReturnCode_t::RETCODE_OK != factory->load_XML_profiles_file("E:\\code\\DDS_Demo\\dds_xml\\domainparticipant_pub_profile.xml")) {
          //  break;
       // }
        return true;
    } while (false);

    return false;
}

MessageSubscriber::MessageSubscriber()
    : participant_(nullptr)
    , type_(new MessagePubSubType())
{
}

MessageSubscriber::~MessageSubscriber()
{
    if (thread_.joinable()) {
        thread_.join();
    }
}

void MessageSubscriber::run(std::string logFile)
{
	std::cout << "Subscriber running. Please press enter to stop the Subscriber" << std::endl;
    listener_->logMsg_.init(logFile);
    listener_->runLog();
}

void MessageSubscriber::run(uint32_t number, std::string logFile)
{
    std::cout << "Subscriber running until " << number << "samples have been received" << std::endl;
}

void MessageSubscriber::stop()
{
    listener_->stopLog();
}


void MessageSubscriber::SubListener::on_data_available(eprosima::fastdds::dds::DataReader* reader)
{
    if (zeroCopy_) {
        zeroCopy(reader);
    }
    else {
        genericCopy(reader);
    }
}

void MessageSubscriber::SubListener::on_subscription_matched(eprosima::fastdds::dds::DataReader* reader, const eprosima::fastdds::dds::SubscriptionMatchedStatus& info)
{
    if (info.current_count_change == 1)
    {
        matched_ = info.total_count;
        std::cout << "Subscriber matched." << std::endl;
    }
    else if (info.current_count_change == -1)
    {
        matched_ = info.total_count;
        std::cout << "Subscriber unmatched." << std::endl;
    }
    else
    {
        std::cout << info.current_count_change
            << " is not a valid value for SubscriptionMatchedStatus current count change" << std::endl;
    }
}

void MessageSubscriber::SubListener::zeroCopy(eprosima::fastdds::dds::DataReader* reader)
{
    // Declare a LoanableSequence for a data type
    FASTDDS_SEQUENCE(DataSeq, Message);

    DataSeq data;
    SampleInfoSeq infos;
    // Access to the collection of data-samples and its corresponding collection of SampleInfo structures
    while (ReturnCode_t::RETCODE_OK == reader->take(data, infos))
    {
        // Iterate over each LoanableCollection in the SampleInfo sequence
        for (LoanableCollection::size_type i = 0; i < infos.length(); ++i)
        {
            // Check whether the DataSample contains data or is only used to communicate of a
            // change in the instance
            if (infos[i].valid_data)
            {
                // Print the data.
                const Message& sample = data[i];
                auto curTime = getTimestamp();
                std::cout << sample.index() << " " << infos[i].reception_timestamp.to_ns() << " " << curTime
                    << " " << infos[i].source_timestamp.to_ns() << " " << message_.timestamp()
                    << " " << infos[i].reception_timestamp.to_ns() - infos[i].source_timestamp.to_ns()
                    << " " << curTime - message_.timestamp() << std::endl;
            }
        }
        // Indicate to the DataReader that the application is done accessing the collection of
        // data values and SampleInfo, obtained by some earlier invocation of read or take on the
        // DataReader.
        reader->return_loan(data, infos);
    }
}

void MessageSubscriber::SubListener::genericCopy(eprosima::fastdds::dds::DataReader* reader)
{
    SampleInfo info;
    while (reader->take_next_sample(&message_, &info) == ReturnCode_t::RETCODE_OK)
    {
        std::this_thread::sleep_for(std::chrono::microseconds(100));
        if (info.instance_state == ALIVE_INSTANCE_STATE)
        {
            samples_++;
            // Print your structure data here.
            auto curTime = getTimestamp();
            logMsg_.appendMsg(message_);
            
            /*
            std::cout << message_.index() << " " << info.reception_timestamp.to_ns() << " " << curTime
                << " " << info.source_timestamp.to_ns() << " " << message_.timestamp()
                << " " << info.reception_timestamp.to_ns() - info.source_timestamp.to_ns()
                << " " << curTime - message_.timestamp() << std::endl;*/
        }
    }
}

LogMsg::~LogMsg()
{
}

void LogMsg::init(std::string logFile)
{
    ofs.open(logFile, std::ios_base::out);
    running_.store(true);
}

void LogMsg::appendMsg(const Message& msg)
{
    time_t curTime = getTimestamp();
    std::unique_lock<std::mutex> locker(mutex_);
    strMsg.append(std::to_string(msg.index()) + "," + std::to_string(msg.timestamp()) + "," + std::to_string(curTime) + "\n");
    if (strMsg.size() >= 1024 * 2) {
        cond_.notify_all();
    }
}

void LogMsg::run()
{
    while (running_.load()) {
        std::unique_lock<std::mutex> locker(mutex_);
        cond_.wait(locker);
        ofs << strMsg;
        //ofs.flush();
        strMsg.clear();
    }
}

void LogMsg::stop()
{
    running_.store(false);
    cond_.notify_all();
}
